# pragma once

# include <vector>
# include <iterator>
# include <cmath>
# include <tuple>
# include <iostream>

# include <cassert>

# include "geometry_details.hpp"



namespace geometry { namespace d3 {

  /*! @brief Class for point presentation
   */
  class Point : public details::CoordinateObject<double> {
  public:

    Point(double x, double y, double z) throw() 
        : details::CoordinateObject<double>(x, y, z) {}

    Point(Point const& p) throw() 
        : details::CoordinateObject<double>(p) {}
      
  };


  /*! @brief Computes distance between points
   */
  inline double distance(Point const& p, Point const& q) throw() {
    double const dx = p.x() - q.x();
    double const dy = p.y() - q.y();
    double const dz = p.z() - q.z();
    return sqrt(dx * dx + dy * dy + dz * dz);
  }


  /*! @brief Class for point presentation
   */
  class Vector: public details::CoordinateObject<double> {
  public:

    Vector(double x, double y, double z) throw() 
        : details::CoordinateObject<double>(x, y, z) {}

    Vector(Vector const& v) throw()
        : details::CoordinateObject<double>(v) {}

    void operator+= (Vector const& v) throw() {
      x() += v.x();
      y() += v.y();
      z() += v.z();
    }

    void operator-= (Vector const& v) throw() {
      x() -= v.x();
      y() -= v.y();
      z() -= v.z();
    }

    void operator*=(double const k) throw() {
      x() *= k;
      y() *= k;
      z() *= k;
    }

    void operator/= (double const k) throw() {
      x() /= k;
      y() /= k;
      z() /= k;
    }

  };


  /*! @brief Operation with vector
   */
  inline Vector operator+(Vector const& p, Vector const& q) throw() {
    return Vector(p.x()+q.x(), p.y()+q.y(), p.z()+q.z());
  }


  /*! @brief Operation with vectors
   */
  inline Vector operator-(Vector const& p, Vector const& q) throw() {
    return Vector(p.x()-q.x(), p.y()-q.y(), p.z()-q.z());
  }


  /*! @brief Operation with vectors
   */
  inline Vector operator*(double k, Vector const& v) throw() {
    return Vector(v.x()*k, v.y()*k, v.z()*k);
  }


  /*! @brief Operation with vectors
   */
  inline Vector operator*(Vector const& v, double k) throw() {
    return Vector(v.x()*k, v.y()*k, v.z()*k);
  }


  /*! @brief Operation on vectors
   */
  inline Vector operator/(Vector const& v, double k) throw() {
    return Vector(v.x()/k, v.y()/k, v.z()/k);
  }


  /*! @brief Inner products of two vectors
   */
  inline double dot(Vector const& u, Vector const& v) throw() {
    return u.x() * v.x() + u.y() * v.y() + u.z() * v.z();
  }


  /*! @brief Computes vector from one point to another
   */
  inline Vector makeVector(Point const& from, Point const& to) throw() {
    return Vector(to.x() - from.x(), to.y() - from.y(), to.z() - from.z());
  }


  /*! @brief Shift point along the vector
   */
  inline Point shiftPoint(Point const& p, Vector const& v) throw() {
    return Point(p.x() + v.x(), p.y() + v.y(), p.z() + v.z());
  }


  /*! @brief Class for plane presentation
   */
  class Plane {
  public:

    /*! @brief Plane constructor
     */
    Plane(double a, double b, double c, double d) throw()
      : cx_(a), cy_(b), cz_(c), c_(d) 
    {
      assert(! ((fabs(cx_) == 0) && (fabs(cy_) == 0) && (fabs(cz_) == 0)));
    }
    
    /*! @brief Plane constructor
     * 
     * @param[in] n normal vector for the plane
     * @param[in] p point in the plane
     */
    Plane(Vector const& n, Point const& p) throw()
      : cx_(n.x()), cy_(n.y()), cz_(n.z()), 
        c_(-n.x()*p.x()-n.y()*p.y()-n.z()*p.z()) 
    {
      assert(! ((fabs(n.x()) == 0) && (fabs(n.y()) == 0) && (fabs(n.z()) == 0)));
    }

    /*! @brief Computes value of the plane on the point
     */
    inline double operator()(Point const& p) const throw() {
      return cx_*p.x()+cy_*p.y()+cz_*p.z()+c_;
    }

  private:

    double cx_;
    double cy_;
    double cz_;
    double c_;

  };


  /*! @brief Line presentation in 3D
   */
  class Line {
  public:

    /*! @brief Line constructor
     *
     * @param[in] tangent tangent vector for the line
     * @param[in] point any point on the line
     *
     * The constructor creates line wich is passing through point p 
     * and is parallel for vector tangent
     */
    Line(Vector tangent, Point point) throw()
      : tangent_(tangent), point_(point) {}

    /*! @brief Line constructor
     *
     * @param[in] fp first point on the line
     * @param[in] sp second point on the line
     */
    Line(Point const& fp, Point sp) throw()
      : tangent_(sp.x() - fp.x(), sp.y() - fp.y(), sp.z() - fp.z()),
        point_(sp) {}

  private:

    /*! @brief Tangent vector for the line
     */
    Vector tangent_;

    /*! @brief Point on the plane
     */
    Point point_;
  };


  /*! @brief Presentation of the coordinate system abstraction
   */
  struct CoordinateSystem {

    CoordinateSystem(Point c, Vector x, Vector y, Vector z)
      : origin(c), ex(x), ey(y), ez(z) {}

    Point origin;
    Vector ex;
    Vector ey;
    Vector ez;

  };


  /*! @brief Point container
   */
  class PointContainer {};


  /*! @brief Computes fitting plane
   */
  CoordinateSystem computeFittingPlane(std::vector<Point> const& cloud);


  /*! @brief Affine transformation of R^3 space
   */
  class AffineTransformation {};

  /*! @brief Solves Procrustes problem
   *
   * @param[in] src source set of points
   * @param[in] dst destination set of points
   * @return affine transformation
   */
  AffineTransformation solveProcrustersProblem(
      std::vector<Point> const& src,
      std::vector<Point> const& dst);


  /*! @brief Morph object
   *
   * @param[in] target target markers location
   * @param[in] model model markers location
   * @param[in] mesh points of the mesh
   * @return morphed mesh
   */
  std::vector<Point> morph(
      std::vector<Point> const& target, 
      std::vector<Point> const& model, 
      std::vector<Point> const& mesh);
  
}}


  /*! @brief Wrapper for translation numbers array to point array
   */
  //class PointContainer {
  //public:

  //  class iterator : public std::iterator<std::bidirectional_iterator_tag, double*> {
  //    friend class PointContainer;
  //  public:

      /*! @brief Copy constructor
       */
  //    iterator(iterator const& it) throw ()
  //      : offset_(it.offset_), itemSize_(it.itemSize_), container_(it.container_) {}

      /*! @brief Assigment operation
       */
  //    iterator& operator=(iterator const& it) throw () {
  //      if (&it != this) {
  //        offset_ = it.offset_;
  //        itemSize_ = it.itemSize_;
  //        container_ = it.container_;
  //      }

  //      return *this;
  //    }

      /*! @brief Equality operation
       */
  //    inline bool operator== (iterator const& it) throw() {
  //      assert(container_ == it.container_);
  //      return offset_ == it.offset_;
  //    }

      /*! @brief Inequality operation
       */
  //    inline bool operator!= (iterator const& it) throw() {
  //      assert(container_ == it.container_);
  //      return offset_ != it.offset_;
  //    }

      /*! @brief Increment operation
       */
  //    inline iterator& operator++() throw () {
  //      assert(offset_+itemSize_ <= container_->size());
  //      offset_ += itemSize_;
  //      return *this;
  //    }

      /*! @brief Increment operation
       */
  //    inline iterator operator++(int) throw() {
  //      assert(offset_+itemSize_ < container_->size());
  //      return iterator(offset_+itemSize_, container_, itemSize_);
  //    }

      /*! @brief Dereferencing
       */
  //    double* operator*() throw() {
  //      return &container_->operator[](0)+offset_;
  //    }

  //  private:

      /*! @brief Class constructor
       */
  //    iterator(size_t const offset, std::vector<double>* container, size_t const sz) throw ()
  //      : offset_(offset), itemSize_(sz), container_(container) {}

  //  private:
  //    size_t offset_;
  //    size_t itemSize_;
  //    std::vector<double>* container_;
  //  };

  //public:

    /*! @brief Class container
     *
     * @param[in] v container with data
     * @param[in] is size of element
     */
  //  explicit PointContainer(std::vector<double>& v, size_t const is) throw () 
  //      : data_(v), itemSize_(is) 
  //  {
  //    assert(v.size() % itemSize_ == 0);
  //  }


    /*! @brief Class constructor
     * 
     * @param[in] v container with data
     * @param[in] is size of elements
     */
   // explicit PointContainer(std::vector<double> const& v, size_t const is) throw()
   //     : data_(const_cast<std::vector<double>&>(v)), itemSize_(is) 
   // {
   //   assert(v.size() % itemSize_ == 0);
   // }

    /*! @brief Get point
     */
   // Point operator[](size_t const index) throw() {
   //   assert(itemSize_*index < data_.size());
   //   return &data_[0]+itemSize_*index;
   // }

    
    /*! @brief Get the fisrt position
     */
   // inline iterator begin() throw() {
   //   return iterator(0, &data_, itemSize_);
   // }

    /*! @brief Get the last position
     */
   // inline iterator end() throw() {
   //   return iterator(data_.size(), &data_, itemSize_);
   // }

  //private:

    /*! @brief Points info
     */
  //  std::vector<double>& data_;

    /*! @brief Elements size
     */
  //  size_t itemSize_;

  //};



  /*! @brief Class for mesh presentation
   */
  //class Mesh {
  //public:

  //  Mesh(std::vector<double>&& points) throw() : points_(points) {}

  //  Mesh(std::vector<double>&& points, std::vector<int>&& edges) throw()
  //    : points_(points), edges_(edges) {}

  //  Mesh(Mesh&& m) : points_(m.points_) {}

    /*! @brief Get point by index
     *
     * @param[in] index 
     * @return point
     */
  //  inline const Point point(size_t const index) const { return const_cast<double*>(&points_[0]+3*index); }

    /*! @brief Get point by index
     *
     * @param[in] index 
     * @return point
     */
  //  inline Point point(size_t const index) { return &points_[0]+3*index; }

    /*! @brief Computes intersection of mesh and the plane
     *
     * @param[in] p plane of intersection
     * @return set of points
     */
  //  std::vector<double> computesIntersection(Plane const& p) const throw();

//# ifdef _DEBUG_2
//    void printPoints() const;
//# endif

//  private:

    /*! @brief Point data
     * 
     * The member contains coordinates of the 3D points
     */
//    std::vector<double> points_;

    /*! @brief Texture data 
     *
     * Container for texture coordinates.
     */
//    std::vector<double> textures_;

    /*! @brief Edges data
     */
//    std::vector<int> edges_;

//  };


  /*! @brief Enum for markers location 
   */
//  enum GlassesMarkers {
//    LEFT_EYE = 0,
//    RIGHT_EYE,
//    LEFT_BRIDGE,
//    RIGHT_BRIDGE,
//    LEFT_TEMPLE,
//    RIGHT_TEMPLE,
//    GlassesMarkers_SIZE
//  };


  /*! @brief Computes bridge section 
   * 
   * @param[in] glasses model of the glasses
   * @return plane of the section
   */
//  Plane computeBridgePlane(Mesh const& glasses);

